package hk.polyu;

import java.awt.Point;
import java.util.*;

public class LocalizationAlgorithm {
	
	Map<Integer, Double>	mBufData;
	
	// Singleton pattern
	private static LocalizationAlgorithm mInst = null;
	private LocalizationAlgorithm()
	{
		mBufData = new TreeMap<Integer, Double>();
	}
	public static LocalizationAlgorithm instance()
	{
		if (mInst == null)
		{
			mInst = new LocalizationAlgorithm();
		}
		return mInst;
	}
	
	public void addData(SensorData d) {
		this.mBufData.put(d.mNodeID, d.mDistance);
	}
	
	public Point getPosition() {
		if (this.mBufData.size() < 3)
			return new Point(0, 0);
		
		Integer [] ids = new Integer[3];
		
		ids = mBufData.keySet().toArray(ids);
		
		Vector<Point> anchors = Setting.instance().mAnchorList;
		
		return Trilateration(anchors.get(0), mBufData.get(ids[0]),
				anchors.get(1), mBufData.get(ids[1]),
				anchors.get(2), mBufData.get(ids[2]) );
		
	}
	
	

	public Point Trilateration(Point p1, double dist1, Point p2, double dist2, Point p3, double dist3) {
		int minX = 0;
		int minY = 0;
		int maxX = 600;
		int maxY = 600;
		
		Point opt = new Point();
		double minVal = Double.MAX_VALUE;
		for(int i = minX; i <= maxX; i++) {
			for(int j = minY; j <= maxY; j++) {
				Point cur = new Point(i, j);
				
				double cVar = Math.pow(cur.distance(p1) - dist1, 2) + Math.pow(cur.distance(p2) - dist2, 2) + Math.pow(cur.distance(p3) - dist3, 2);
				
				if (cVar < minVal) {
					minVal = cVar;
					opt = cur;
				}
			}
		} // end outer for.
		return opt;
	}

}
